#include "Windmill.h"
#include"../General/opencv_extended.h"
#include"../General/numeric_rm.h"

namespace  rm
{

Windmill::Windmill()
{
    hitNumber=0;           //设置击打次数
    _isfund=false;         //初始化是否寻找到打击中心标志量为false
    _isfund_R=false;
    Lead=1;                //set the lead
    OFFSET=20;
    DILATENUM = 5;         //设置膨胀系数
    MINRANGE_SWATTER = 1800;   //设置最小边框检测范围
    MAXRANGE_SWATTER = 3500;   //设置最大边框检测范围
    MINRANGE_R=150;
    MAXRANGE_R=350;
    MINRANGE_FAN=10000;
    MAXRANGE_FAN=11000;
    MINRANGE_RECTANGE=3100;
    MAXRANGE_RECTANGE=3500;

    TARGETTHRESH = 160;    //设置目标颜色阈值
    ELSETHRESH = 200;      //设置其他颜色阈值

    COMRETHRESH = 10;      //设置形状符合阈值
    FRAMENUMBER = 80;      //设置每秒帧数---------------------------
    BULLETSPEED = 18;//28;      //设置子弹初速度
    BUFFTYPETHRESH = 100000;   //设置大小符的阈值(to do...)
    colorNumber = 0;
    test_num = 0;
    _DISTANCE = 7;
    min_distance=8;//original 18
    max_distance=280;//original:38
    //----------------------
    //initialize previous angle&scalar
    buff_type = -1;
    _isclockwise = -1;

    for(int i = 0; i < QUEUE_NUM; i++)
    {
        pre_sinangle[i]=0;
    };
    pre_angle_head=0;
    pre_angle_tail=0;
    pre_direction.x=0;
    pre_direction.y=0;
    _pre_angle = -1;
    //----------------------

    start_time = 0;
    pre_time = -1;
    prepre_time = -1;
    max_omega1 = 0;
    max_omega1_time = 0;
    max_omega1_find = 0;
    min_omega = 10000;
    is_accelerated = 0;
    refresh_time = 0;

    predict_done = false;
    phi = 0;

    omegas.clear();
}

Windmill::~Windmill()
{

}
void Windmill::refresh()
{
    buff_type = -1;
    _isclockwise = -1;
    _framecounter = 0;

    for(int i = 0; i < QUEUE_NUM; i++)
    {
        pre_sinangle[i]=0;
    };
    pre_angle_head=0;
    pre_angle_tail=0;
    pre_direction.x=0;
    pre_direction.y=0;
    _pre_angle = -1;
    //----------------------

    start_time = 0;
    pre_time = -1;
    prepre_time = -1;
    max_omega1 = 0;
    max_omega1_time = 0;
    max_omega1_find = 1;
    min_omega = 10000;
    is_accelerated = 0;
    predict_done = false;
    omegas.clear();
    phi = 0;

    refresh_time++;
}
void Windmill::setColor(int i)
{
    colorNumber = i;
}
double Windmill:: Max(double a, double b, double c)
{
    double x = a;
    if (a > b)
        x = a;
    else
        x = b;
    if (x > c)
        x = x;
    else
        x = c;
    return x;
}
double Windmill::Min(double a, double b, double c)
{
    double x = a;
    if (a < b)
        x = a;
    else
        x = b;
    if (x < c)
        x = x;
    else
        x = c;
    return x;
}
void Windmill::loadImg(Mat frame_, double time)
{
    _srcImg.srcImg = frame_.clone();
    _srcImg.time = time/1000; //ms->s

    if(_isfund&&_isfund_R)
    {
        double R_Fan_distance = sqrt(pow((newDirection.x-_center.x),2)+pow((newDirection.y-_center.y),2));
        if(R_Fan_distance > 35||R_Fan_distance<5)
        {
            _srcImg.roi = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
            _srcImg.roi_R = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
        }
        else
        {
            _srcImg.roi = Rect((_center-Point2f(200, 200)),Size(400,400)) & Rect(cv::Point(0, 0), _srcImg.srcImg.size());
            _srcImg.roi_R = Rect((_center-Point2f(25, 25)),Size(50,50)) & Rect(cv::Point(0, 0), _srcImg.srcImg.size());
        }
        double R_Fancenter_distance = sqrt(pow((_fanCenter.x-_center.x),2)+pow((_fanCenter.y-_center.y),2));
        if(R_Fancenter_distance>155||R_Fancenter_distance<70)
        {
            _srcImg.roi = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
            _srcImg.roi_R = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
        }
        else
        {
            _srcImg.roi = Rect((_center-Point2f(200, 200)),Size(400,400)) & Rect(cv::Point(0, 0), _srcImg.srcImg.size());
            _srcImg.roi_R = Rect((_center-Point2f(25, 25)),Size(50,50)) & Rect(cv::Point(0, 0), _srcImg.srcImg.size());
        }
    }
    else
    {
        _srcImg.roi = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
        _srcImg.roi_R = Rect(cv::Point(0, 0), _srcImg.srcImg.size());
    }
    _srcImg.roiImg = _srcImg.srcImg(_srcImg.roi).clone();
    _srcImg.roiImg_R = _srcImg.srcImg(_srcImg.roi_R).clone();
    rectangle(_srcImg.srcImg, _srcImg.roi, cvex::YELLOW);
    rectangle(_srcImg.srcImg, _srcImg.roi_R, cvex::YELLOW);
}

float Windmill::distance(Point2f Pointa, Point2f Pointb)
{
    float result = 0;
    result = hypotf(Pointa.x - Pointb.x, Pointa.y - Pointb.y);
    return result;
}

void Windmill::readFile()
{
    ifstream file_fan,file_R;

    file_fan.open("/home/ssj5vkjcty/Robomaster2022/Windmill/1.txt");

    file_R.open("/home/ssj5vkjcty/Robomaster2022/Windmill/R.txt");

    string str;

    //存储模板
    string a, b;
    int p;
    while (getline(file_fan, str))   //扇叶的轮廓匹配
    {
        p = str.find(",");
        a = str.substr(1, p - 1);
        b = str.substr(p + 2);
        b = b.substr(0, b.size() - 1);
        int x = atoi(a.c_str());
        int y = atoi(b.c_str());
        templateContour_swatter.push_back(Point(x, y));
    }
    while (getline(file_R, str))       //R的轮廓匹配
    {
        p = str.find(",");
        a = str.substr(1, p - 1);
        b = str.substr(p + 2);
        b = b.substr(0, b.size() - 1);
        int x = atoi(a.c_str());
        int y = atoi(b.c_str());
        templateContour_R.push_back(Point(x, y));
    }
}

void Windmill::Detect()
{
    //分离三通道的图像，然后使用不同的阈值二值化
//    Mat _gray(_srcImg.srcImg.size(), CV_8UC1);
//    cvtColor(_srcImg.srcImg, _gray, COLOR_BGR2GRAY, 1);
//    threshold(_gray, _processedImg, TARGETTHRESH, 255, THRESH_BINARY);

    vector<Mat> channels;
    Mat RedImg(_srcImg.roiImg.size(), CV_8U, Scalar(0)), threRedImg(_srcImg.roiImg.size(), CV_8U, Scalar(0));
    Mat BlueImg(_srcImg.roiImg.size(), CV_8U, Scalar(0)), threBlueImg(_srcImg.roiImg.size(), CV_8U, Scalar(0));
    //Mat GreenImg(_srcImg.roiImg.size(), CV_8U, Scalar(0)), threGreenImg(_srcImg.roiImg.size(), CV_8U, Scalar(0));
    split(_srcImg.roiImg, channels);
    BlueImg = channels.at(0);
    //GreenImg = channels.at(1);
    RedImg = channels.at(2);
    if (colorNumber==2)
    {
        threshold(BlueImg, threBlueImg, ELSETHRESH, 255, THRESH_BINARY_INV);
        threshold(RedImg, threRedImg, TARGETTHRESH, 255, THRESH_BINARY);
        //threshold(GreenImg, threGreenImg, ELSETHRESH, 255, THRESH_BINARY_INV);
    }
    if (colorNumber==0)  // blue
    {
        threshold(BlueImg, threBlueImg, TARGETTHRESH, 255, THRESH_BINARY);
        threshold(RedImg, threRedImg, ELSETHRESH, 255, THRESH_BINARY_INV);
        //threshold(GreenImg, threGreenImg, ELSETHRESH, 255, THRESH_BINARY_INV);
    }

    vector<Mat> channels_R;
    Mat RedImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0)), threRedImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0));
    Mat BlueImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0)), threBlueImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0));
    //Mat GreenImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0)), threGreenImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0));
    split(_srcImg.roiImg_R, channels_R);
    BlueImg_R = channels_R.at(0);
    //GreenImg_R = channels_R.at(1);
    RedImg_R = channels_R.at(2);
    if (colorNumber==2)
    {
        threshold(BlueImg_R, threBlueImg_R, ELSETHRESH, 255, THRESH_BINARY_INV);
        threshold(RedImg_R, threRedImg_R, TARGETTHRESH, 255, THRESH_BINARY);
        //threshold(GreenImg_R, threGreenImg_R, ELSETHRESH, 255, THRESH_BINARY_INV);
    }
    if (colorNumber==0)  // blue
    {
        threshold(BlueImg_R, threBlueImg_R, TARGETTHRESH, 255, THRESH_BINARY);
        threshold(RedImg_R, threRedImg_R, ELSETHRESH, 255, THRESH_BINARY_INV);
        //threshold(GreenImg_R, threGreenImg_R, ELSETHRESH, 255, THRESH_BINARY_INV);
    }

    Mat _processedImg(_srcImg.roiImg.size(), CV_8U, Scalar(0));
    Mat _processedImg_R(_srcImg.roiImg_R.size(), CV_8U, Scalar(0));
    //imshow("Blue channel binary", threBlueImg);
    //imshow("Green channel binary", threGreenImg);
    //imshow("Red channel binary", threRedImg);
    //合并图像

    Mat_ <uchar>::const_iterator it2 = threBlueImg.begin<uchar>();
    Mat_ <uchar>::const_iterator it0 = threRedImg.begin<uchar>();
    Mat_ <uchar>::const_iterator itend = threBlueImg.end<uchar>();
    Mat_ <uchar>::iterator itout = _processedImg.begin<uchar>();
    for (; it2 != itend; /*++it1,*/ ++it2, ++it0, ++itout)
    {
        if(/**it1 == 255 &&*/ *it2 == 255 && *it0 == 255)
        {
            *itout = 255;
        }
        else
        {
            *itout = 0;
        }
    }
    //Mat_ <uchar>::const_iterator it1_R = threGreenImg_R.begin<uchar>();
    Mat_ <uchar>::const_iterator it2_R = threBlueImg_R.begin<uchar>();
    Mat_ <uchar>::const_iterator it0_R = threRedImg_R.begin<uchar>();
    Mat_ <uchar>::const_iterator itend_R = threBlueImg_R.end<uchar>();
    Mat_ <uchar>::iterator itout_R = _processedImg_R.begin<uchar>();
    for (; it2_R != itend_R; /*++it1_R,*/ ++it2_R, ++it0_R, ++itout_R)
    {
        if(/**it1_R == 255 &&*/ *it2_R == 255 && *it0_R == 255)
        {
            *itout_R = 255;
        }
        else
        {
            *itout_R = 0;
        }
    }
    //通过膨胀使得轮廓更加清晰
    //Mat element1 = getStructuringElement(MORPH_RECT,Size(1,1));
    //erode(_processedImg,_processedImg,element1);
    //waitKey(1);


    Mat element2 = getStructuringElement(MORPH_RECT,Size(DILATENUM,DILATENUM));
    dilate(_processedImg,_processedImg,element2);

    Mat element2_R = getStructuringElement(MORPH_RECT,Size(DILATENUM,DILATENUM));
    dilate(_processedImg_R,_processedImg_R,element2_R);
    imshow("dilate imshow", _processedImg);
    waitKey(1);


//#ifdef DEBUG

//#endif

    //将要打击的装甲板的颜色要比已经打击的装甲板更亮，所以经过滤波之后已经打击过的装甲板会有一部分成分的残缺，这不用管他，可以还会对后面的识别有所帮助。
    findContours(_processedImg, contours, hierachy, RETR_CCOMP, CV_CHAIN_APPROX_NONE);
    findContours(_processedImg_R, contours_R, hierachy_R, RETR_CCOMP, CV_CHAIN_APPROX_NONE);
//    for(auto _contours:contours)
//    {
//        RotatedRect MillRec = fitEllipse(_contours);
//        Rect MillRect =MillRec.boundingRect();
//        Mat MilltMask = Mat::zeros(MillRect.size(), CV_8UC1);
//        const Scalar meanVal = mean(_srcImg.srcImg, MilltMask);
//        if(((colorNumber == 0) && (meanVal[0] - meanVal[2] > 35.0) && (meanVal[1] < 125.0)))
//        {

//        }
//        if((colorNumber == 2 && (meanVal[2] - meanVal[0] > 17.0)&& (meanVal[1] < 125.0) ))
//        {

//        }
//    }


//#ifdef DEBUG

//    for(int i = 0; i<contours.size(); i++)
//    {
//        drawContours(_processedImg, contours,22, Scalar(100), 3);
//        cout<<i<<"    "<<contourArea(contours.at(i))<<endl;
//        imshow("contours imshow", _processedImg);
//        waitKey(0);
//    }

    //drawContours(_srcImg.srcImg, contours,-1, Scalar(100), 3);
//    imshow("contours imshow", _processedImg);
//    waitKey(1);

//    ofstream o;
//    o.open("/home/ssj5vkjcty/R.txt", ios::app);
//    for(int i = 0;i<contours.at(22).size();i++)
//    {
//        o<<" "<<contours.at(22).at(i).x<<", "<<contours.at(22).at(i).y<<";"<<endl;
//    }
//#endif

//    cout<<endl<<"CounterArea："<<endl;
//    for(int i =0;i<contours.size();++i)
//    {
//        cout<<contourArea(contours[i])<<"  ";
//    }
//    cout<<endl;

    Fan_recognition(templateContour_swatter);
    R_recognition(templateContour_R);
    returnNextPoint();
    imshow("读取视频1", _srcImg.srcImg);
    waitKey(1);
    Whether_To_Hit(contours);
}


Point2f Windmill:: forecastNextPoint(Point2f pt1, Point2f pt2)
{
    Point2f fanCenter=pt1;
    Point2f center=pt2;
    double cos_angle = 1;
    double angle = 0;
    float offset = OFFSET;


    if(_isfund)//没有掉帧
    {
        //计算现在画面矢量
#ifdef DEBUG_FORC
        //cout << " 扇叶重心： " << pt1 << endl;
#endif
#ifdef DEBUG_FORC
        //cout << " 扇叶最远点： " << pt2 << endl;
#endif
        length=distance(fanCenter, center);
        direction.x=fanCenter.x-center.x;
        direction.y=-(fanCenter.y-center.y);    //将图片坐标变为正常的坐标（y轴正方向由向下变为向上）
        //determine _isclockwise
        if(_framecounter == 0)
        {
            _first_direction.x = direction.x;
            _first_direction.y = direction.y;
            start_time = _srcImg.time;
        }

        if(_framecounter < QUEUE_NUM)
        {
            _framecounter++;
            //cout<<"_framecounterjjjj:"<<_framecounter<<endl;
        }

        if(_framecounter == 5)
        {
            Lead = _first_direction.x*direction.y-direction.x*_first_direction.y;
            if(_first_direction.x*direction.y-direction.x*_first_direction.y > 0)
            {
                _isclockwise=0;//anticlockwise
            }
            else
            {
                _isclockwise=1;
            }
        }

        //cout<<"clock.........................."<<_isclockwise<<endl;
        if(_framecounter < QUEUE_NUM)
        {
            //计算最近一次的角度改变量
            if (pre_direction.x != 0 || pre_direction.y != 0)
            {
                cos_angle=(pre_direction.x*direction.x+pre_direction.y*direction.y)
                        /(sqrt(pow(direction.x,2)+pow(direction.y,2))*sqrt(pow(pre_direction.x,2)+pow(pre_direction.y,2)));
                if(cos_angle < 1)
                {
                    angle = acos(cos_angle);
                }
                else
                {
                    angle=0;
                }
                if( ( (angle/(_srcImg.time-pre_time)) > max_omega1)/* && max_omega1_find == 0*/)
                {
                    if(angle/(_srcImg.time-pre_time) < 3 && abs((angle/(_srcImg.time-pre_time)) - (_pre_angle/(pre_time-prepre_time))) < 1)
                    {
                        max_omega1 = angle/(_srcImg.time-pre_time);
                        max_omega1_time = _srcImg.time;
                    }
                }

                if( (angle/(_srcImg.time-pre_time)) < min_omega && _framecounter < QUEUE_NUM)
                {
                    min_omega = angle/(_srcImg.time-pre_time);
                }
//                if(_pre_angle > 0)
//                {
//                    if( (_pre_angle/(pre_time-prepre_time) < angle/(_srcImg.time-pre_time)) && is_accelerated == 0)
//                    {
//                        if(angle/(_srcImg.time-pre_time)>0.5)
//                        {
//                            is_accelerated = 1;
//                        }
//                    }
//                    if( (_pre_angle/(pre_time-prepre_time) > (angle/(_srcImg.time-pre_time))+0.01) && max_omega1_find == 0 && is_accelerated == 1)
//                    {
//                        if(angle/(_srcImg.time-pre_time)>0.5)
//                        {
//                            max_omega1_find = 1;
//                        }
//                    }
//                }
            }
            if(_srcImg.time-pre_time > 0.3 /*|| cos_angle < 0.7*/)
            {
                refresh();
                refresh_time += 0.1;
                cout<<"111111111111111111111\n22222222222222222222\n3333333333333333333\n44444444444444444444"<<endl;
                //cout<<_srcImg.time-pre_time<<"   ,   "<< cos_angle<<endl;
            }
        }

        //update previous_direction
        //cout << "omega:" << (angle/(_srcImg.time-pre_time)) <<endl;
        if(pre_time != _srcImg.time && cos_angle > 0.7)
        {
            if(abs((angle/(_srcImg.time-pre_time)) - (_pre_angle/(pre_time-prepre_time))) < 1)
            {
                omegas.push_back(Point2d(_srcImg.time - start_time, angle/(_srcImg.time-pre_time)));
            }
            _pre_angle = angle;
            pre_direction.x=direction.x;
            pre_direction.y=direction.y;
            prepre_time = pre_time;
            pre_time = _srcImg.time;
        }
    }

    float forecast_angle = 0;

#ifdef DEBUG_FORC
    //cout << endl;
    //cout << "direction:(" << direction.x << "," << direction.y << ")";
    //cout << "firstDir: (" << _first_direction.x << "," <<_first_direction.y << ")"<<endl;
    //cout << "fancenter:( " << pt1.x << "," << pt1.y << ")   ";
    //cout << "center:( " << pt2.x << "," << pt2.y << ")   " << endl;
    cout << "max_omega1: " << max_omega1 << endl;
    cout << "min_omega: " << min_omega << endl;
    //cout << "length:" << length << endl;
    //cout << "start time:  " << start_time << endl;
    //cout << "now time:  " << _srcImg.time << endl;
    //cout << "lead" << Lead << endl;
    //cout << "refresh_time" << refresh_time << endl;
    //cout << endl;
#endif


    if(_framecounter == QUEUE_NUM )//队满开始预测
    {

//-------------------------------------------------------------------自动判断大小幅
        if(!predict_done)
        {
            if(max_omega1 - min_omega > 2)
            {
               buff_type = 1;
               //cout<<"bigbuff"<<endl;
            }
            else
            {
               buff_type = 0;
               //cout<<"smallbuff"<<endl;
            }
            show_omegas();
            //phi = /*3*/CV_PI/2 - 1.884*(max_omega1_time-start_time) + 0.02;
            phi = fit_phi(omegas);
            predict_done = true;
        }
//-------------------------------------------------------------------
        if (buff_type == 0)//小符
        {
            forecast_angle = (CV_PI / 3) * 12/ BULLETSPEED ;
        }
        else if(buff_type == 1)//大符
    //大符的运动公式：spd = 0.785sin(1.884t + φ) + 1.305, diff(spd)/diff(t) = 1.479cos(1.884t + φ)
        {
            if(max_omega1_find)
            {
                //cout<<"========================================="<<endl;
                //cout<<"phi: "<<phi<<endl;
                //cout<<"========================================="<<endl;
                double forecast_time = 11 / BULLETSPEED;
//                forecast_angle = (0.785*sin(1.884*(_srcImg.time-start_time) + phi) + 1.305)*forecast_time +
//                        0.5*1.479*cos(1.884*(_srcImg.time-start_time))*forecast_time*forecast_time + 0.1; // rad
                forecast_angle = 0.4167*cos(1.884*(_srcImg.time-start_time) + phi) - 0.4167*cos(1.884*(_srcImg.time-start_time+forecast_time) + phi) + 1.305*forecast_time;
                //forecast_angle *= (0.785*sin(1.884*(_srcImg.time-start_time) + phi) + 0.785)*(0.15/0.785) + 1;

                //forecast_angle -= -0.09;


//                if(compensateangle > 1.5*PI || compensateangle <= PI/2)
//                    offset -= 2;


                //cout<<compensate.x<<endl;
            }
            else
            {
                refresh();
                refresh_time+=0.01;
            }
        }
    }

    //计算现在画面中打击点位置
    if(_isfund)
    {
        Point2f _direction(0, 0);
        double _length=distance(fanCenter, center);
        _direction.x=fanCenter.x-center.x;
        _direction.y=-(fanCenter.y-center.y);

        float sinDirection = _direction.y/_length;
        float cosDirection = _direction.x/_length;
        float processy=offset * fanwidth * sinDirection;
        float processx=offset * fanwidth * cosDirection;
        Point2f forecastPoint;
        forecastPoint.x=fanCenter.x+processx;
        forecastPoint.y=-fanCenter.y+processy;
        forecastPoint.y=-forecastPoint.y;
#ifdef DEBUG
//        circle(_srcImg.srcImg,  forecastPoint, 10, Scalar(255, 0,255),-1);
#endif
        //计算预测位置
        Point2f forecastPointMid;
        Point2f forecastPointPlus;
        if (_framecounter == QUEUE_NUM)//队满才预测
        {
            forecastPointMid.x=(center.x+cosDirection*(_length+offset)*cos(forecast_angle));
            forecastPointMid.y=(-center.y+sinDirection*(_length+offset)*cos(forecast_angle));
            if(_isclockwise == 1)//shun
            {
                forecast_angle = -forecast_angle;
                forecastPointPlus.x=(forecastPointMid.x-sinDirection*(_length+offset)*sin(forecast_angle));
                forecastPointPlus.y=-(forecastPointMid.y+cosDirection*(_length+offset)*sin(forecast_angle));
            }
            if(_isclockwise == 0)//ni
            {
                forecastPointPlus.x=(forecastPointMid.x-sinDirection*(_length+offset)*sin(forecast_angle));
                forecastPointPlus.y=-(forecastPointMid.y+cosDirection*(_length+offset)*sin(forecast_angle));     //负号改回
            }
            forecastPointMid.y = -forecastPointMid.y;

#ifdef DEBUG
//            circle(_srcImg.srcImg, forecastPointMid, 7, Scalar(0,0,255),-1);
//            circle(_srcImg.srcImg, forecastPointPlus, 5, Scalar(255, 255,0),-1);
//        cout << "----------------------------------forecast_angle： " << forecast_angle << endl;
//            cout <<"forecastPointMid: "<<forecastPointMid << "      ";
//            cout<<"the center of forecast is"<<forecastPointPlus<<endl;
#endif
            return forecastPointPlus;
        }
        else
        {
            Point2f d(0,0);
            return d;
        }
    }
    else
    {
        Point2f d(0,0);
        return d;
    }
}


Point2f Windmill::returnNextPoint()
{
    //----------------------------------------------------------
//    if(_isfund_R)
//    {
//        _forecastCenter=forecastNextPoint(_fanCenter,_center);
//    }
//    else
//    {
//        _forecastCenter=forecastNextPoint(_fanCenter,newDirection);//NO R
//    }
    _forecastCenter=forecastNextPoint(_fanCenter,_center);
    //----------------------------------------------------------
    double centerdistance;
    centerdistance=sqrt((_fanCenter.x-_forecastCenter.x)*(_fanCenter.x-_forecastCenter.x)+(_fanCenter.y-_forecastCenter.y)*(_fanCenter.y-_forecastCenter.y));
    if(centerdistance>max_distance||centerdistance<min_distance)
    {
        Point2f d(0,0);
        _forecastCenter =  d;
    }
    double R_Fan_distance = sqrt(pow((newDirection.x-_center.x),2)+pow((newDirection.y-_center.y),2));
    if(R_Fan_distance > 45||R_Fan_distance<2)
    {
        Point2f d(0,0);
        _forecastCenter =  d;
        //cout<<"wrong shape1   "<<_srcImg.time<<endl;
    }
    double R_Fancenter_distance = sqrt(pow((_fanCenter.x-_center.x),2)+pow((_fanCenter.y-_center.y),2));
    if(R_Fancenter_distance>155||R_Fancenter_distance<40)
    {
        Point2f d(0,0);
        _forecastCenter =  d;
        //cout<<"wrong shape2   "<<_srcImg.time<<endl;
    }
//    cout<<"R_Fancenter_distance"<<R_Fancenter_distance<<endl;
//    cout<<"R_Fan_distance"<<R_Fan_distance<<endl;
//    cout<<"centerdistance"<<centerdistance<<endl;

    circle(_srcImg.srcImg, _forecastCenter, 5, Scalar(0,155,255),-1);
    return _forecastCenter;  //如果可以预测便返回这个Point2f类型的预测点
}

Point2f Windmill::getNextPoint()
{
    return _forecastCenter;
}

bool Windmill::Whether_To_Hit(vector<vector<Point>> contours)    //判断是否要进行击打
{
    int rectangleNum=0;
    for(int i=0;i<contours.size();++i)
    {
        if (contourArea(contours[i]) > MINRANGE_FAN && contourArea(contours[i]) < MAXRANGE_FAN)
        {
            ++hitNumber;
            //cout<<endl<<"going to hit"<<endl;
            return true;
            //--///////////////////////////////////Problem: 需要控制击打的时间间隔
        }
        if (contourArea(contours[i]) > MINRANGE_RECTANGE && contourArea(contours[i]) < MAXRANGE_RECTANGE)
        {
            ++rectangleNum;
        }
    }

    if(rectangleNum==5)
    {
        //cout<<"stop hitting"<<endl;
        return false;
    }

}

void Windmill::Fan_recognition(vector<Point> templateContour)    //识别苍蝇拍
{
    vector<float> comres_swatter;
    for(int i = 0; i<contours.size(); ++i)
    {
        if (contourArea(contours[i]) < MINRANGE_SWATTER||contourArea(contours[i]) > MAXRANGE_SWATTER)
        {
            comres_swatter.push_back(9999);
            continue;
        }
        else
            comres_swatter.push_back(matchShapes(contours[i],templateContour,CV_CONTOURS_MATCH_I1,0.0));
    }
    int NUM = comres_swatter.size();

    float MinTemp = 99999;
    int Record=0;
    /*
        cout<<endl<<"轮廓的匹配度"<<endl;
        for(int i=0;i<comres.size();++i)
        {
            cout<<comres[i]<<"  ";
        }
        cout<<endl;
*/

    cout<<comres_swatter.size()<<endl;
    if(comres_swatter.size())
    {
        MinTemp=comres_swatter[0];
        for( int t = 0;t < NUM;t++)
        {
            if(comres_swatter[t]<MinTemp)
            {
                Record=t;
                MinTemp=comres_swatter[t];
            }
        }
        cout<<"comres_swatter.....fan"<<comres_swatter[Record]<<endl;
        if(comres_swatter[Record]<5)
        {
//            drawContours(_srcImg.srcImg, contours,Record, Scalar(180), 3);
//            drawContours(_srcImg.srcImg, contours,hierachy.at(Record)[2], Scalar(150), 3);
            if(contours.size() > hierachy.at(Record)[2])
            {
                if(contourArea(contours.at(hierachy.at(Record)[2])) > 180)
                {
                    RotatedRect _armor_inside_fan = minAreaRect(contours.at(hierachy.at(Record)[2]));

                    float width = _armor_inside_fan.size.width > _armor_inside_fan.size.height ? _armor_inside_fan.size.width : _armor_inside_fan.size.height;
                    float height = _armor_inside_fan.size.width > _armor_inside_fan.size.height ? _armor_inside_fan.size.height : _armor_inside_fan.size.width;
                    if(height == 0)
                    {
                        width = 1;
                        height = 1;
                    }
                    if(width/height <= 3)
                    {
                        armor_inside_fan = minAreaRect(contours.at(hierachy.at(Record)[2]));
                    }
                    Point2f vertex[4];
                    armor_inside_fan.points(vertex);
                    for(int i = 0; i < 4; i++)
                    {
                        line(_srcImg.srcImg, vertex[i] + Point2f(_srcImg.roi.tl()), vertex[(i+1)%4] + Point2f(_srcImg.roi.tl()), Scalar(100, 200, 211), 2, LINE_AA);
                    }
                }
            }
            //cout<<"comres_swatter::::::::::" << comres_swatter[Record]<<endl;
            //计算重心
            Moments mu = moments(contours[Record],false);
            _fanCenter = Point2f(mu.m10/mu.m00 , mu.m01/mu.m00);


            double newLength = 0.0;

            //标记离质心最远的点
            int flag = 0;
            for (int j = 0; j<contours[Record].size(); j++)
            {
                float temporaryLength = distance(contours[Record][j],_fanCenter);
                if(temporaryLength > newLength)
                {
                    newLength=temporaryLength;
                    flag = j;
                }
            }
            if(flag < (contours[Record].size() - 2)&&flag > 0)
            {
                newDirection = (contours[Record][flag-1] + contours[Record][flag+1] +contours[Record][flag])/3;
            }
            else if(flag > 2)
            {
                newDirection = (contours[Record][flag-1] + contours[Record][flag-2] +contours[Record][flag])/3;
            }
            else if(flag < (contours[Record].size() - 3))
            {
                newDirection = (contours[Record][flag+1] + contours[Record][flag+2] +contours[Record][flag])/3;
            }
            else
                newDirection = contours[Record][flag];

            _fanCenter += Point2f(_srcImg.roi.tl());
            circle(_srcImg.srcImg,_fanCenter,3,Scalar(100,0,100),3);
            newDirection += Point2f(_srcImg.roi.tl());
            circle(_srcImg.srcImg,newDirection,1,Scalar(0,0,100),5);
            _isfund = true;
            /*
                cout << "length:" << length << endl;
                cout << "offset:" << OFFSET <<endl;
                cout << "contourArea: " << contourArea(contours[Record]) << endl;
                cout << "comres:" << comres[Record]<<endl;
*/

        }
        else
            _isfund = false;
    }
}

void Windmill::R_recognition(vector<Point> templateContour)        //识别R
{
    vector<float> comres_R;
    int NUM = comres_R.size();
    float MinTemp = 99999;
    int Record=0,location=0;
    for(int i=0;i<contours_R.size();++i)
    {
        if (contourArea(contours_R[i]) < MINRANGE_R||contourArea(contours_R[i]) > MAXRANGE_R)
        {
            comres_R.push_back(9999999);
            continue;
        }
        else
        {
            comres_R.push_back(matchShapes(contours_R[i],templateContour,CV_CONTOURS_MATCH_I1,0.0));
            location=i;
        }
    }

    NUM = comres_R.size();

    //cout<<endl;
    if(comres_R.size())
    {
        MinTemp=comres_R[0];
        for( int t = 0;t < NUM;t++)
        {
            if(comres_R[t]<MinTemp)
            {
                Record=t;
                MinTemp=comres_R[t];
            }
        }
        //cout<<endl;
        cout<<"comres_R::::::::::" << comres_R[Record]<<endl;
        if(comres_R[Record]<3)
        {
            cout<<"comres_R::::::::::" << comres_R[Record]<<endl;
            Moments mu1 = moments(contours_R[Record],false);
            _center = Point2f(mu1.m10/mu1.m00 , mu1.m01/mu1.m00);
            _center += Point2f(_srcImg.roi_R.tl());
            circle(_srcImg.srcImg,_center,1,Scalar(55,55,55),3);
            //cout<<_center<<endl;
           // Point2f begin_point(0,comres_R.at(1));
           // Point2f end_point(500,comres_R.at(1));
           // line(_srcImg.srcImg,begin_point,end_point,Scalar(0,0,255),2);
            _isfund_R = true;
        }
        else
        {
            cout<<"comres_R::::::::::::::::::::::::::::" << comres_R[Record]<<endl<<"not_found"<<endl;
            _isfund_R = false;
        }
    }
}

double Windmill::fit_phi(vector<Point2d> points)//最小二乘法,拟合三次函数
{
    int size = points.size();
    //所求未知数个数
    int x_num = 4;
    //构造矩阵U和Y
    Mat mat_u(size, x_num, CV_64F);
    Mat mat_y(size, 1, CV_64F);

    for (int i = 0; i < mat_u.rows; ++i)
        for (int j = 0; j < mat_u.cols; ++j)
        {
            mat_u.at<double>(i, j) = pow(points[i].x, j);
        }

    for (int i = 0; i < mat_y.rows; ++i)
    {
        mat_y.at<double>(i, 0) = points[i].y;
    }

    //矩阵运算，获得系数矩阵K
    Mat mat_k(x_num, 1, CV_64F);
    solve(mat_u.t()*mat_u, mat_u.t()*mat_y, mat_k);
    //cout << mat_k << endl;

    double k1 = mat_k.at<double>(3, 0);
    double k2 = mat_k.at<double>(2, 0);
    double k3 = mat_k.at<double>(1, 0);

    if(k2*k2-3*k1*k3 <= 0)
    {
        return 0;
    }
    double x_max;
    if(k1 > 0)
    {
        x_max = (sqrt(k2*k2-3*k1*k3) - k2)/(3*k1);
    }
    if(k1 == 0)
    {
        return 0;
    }
    if(k1 < 0)
    {
        x_max = (-sqrt(k2*k2-3*k1*k3) - k2)/(3*k1);
    }
    return /*3**/CV_PI/2 - 1.884*(x_max);
}

const std::vector<cv::Point2f> Windmill::getArmorInsideVertex() const //tl, tr, br, bl
{
    Point2f vertex[4];
    armor_inside_fan.points(vertex);

    vector<cv::Point2f> realVertex;
    if(armor_inside_fan.size.width > armor_inside_fan.size.height)
    {
        realVertex.emplace_back(Point2f(vertex[1].x + _srcImg.roi.tl().x, vertex[1].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[2].x + _srcImg.roi.tl().x, vertex[2].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[3].x + _srcImg.roi.tl().x, vertex[3].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[0].x + _srcImg.roi.tl().x, vertex[0].y + _srcImg.roi.tl().y));
    }
    else
    {
        realVertex.emplace_back(Point2f(vertex[0].x + _srcImg.roi.tl().x, vertex[0].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[1].x + _srcImg.roi.tl().x, vertex[1].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[2].x + _srcImg.roi.tl().x, vertex[2].y + _srcImg.roi.tl().y));
        realVertex.emplace_back(Point2f(vertex[3].x + _srcImg.roi.tl().x, vertex[3].y + _srcImg.roi.tl().y));
    }
    return realVertex;
}

const RotatedRect Windmill::getArmorInside() const
{
    return armor_inside_fan;
}

void Windmill::show_omegas()
{
    Mat out(600, 1000, CV_8UC3,Scalar::all(0));

    circle(out, Point2i(omegas.at(0).x*250, omegas.at(0).y*100 + 20), 3, Scalar(0, 120, 255), CV_FILLED, CV_AA);
    putText(out, to_string(omegas.at(0).x) + ", " + to_string(omegas.at(0).y),
            Point2i(omegas.at(0).x*250 + 5, omegas.at(0).y*100 + 5 + 20), FONT_HERSHEY_PLAIN, 1, Scalar(0,195,0), 1);

    for(int i = 1; i < omegas.size(); i++)
    {
        circle(out, Point2i(omegas.at(i).x*250, omegas.at(i).y*100 + 20), 3, Scalar(0, 120, 255), CV_FILLED, CV_AA);
        putText(out, to_string(omegas.at(i).x) + ", " + to_string(omegas.at(i).y),
                Point2i(omegas.at(i).x*250 + 5, omegas.at(i).y*100 + 5 + 20), FONT_HERSHEY_PLAIN, 1, Scalar(0,195,0), 1);
        line(out, Point2i(omegas.at(i-1).x*250, omegas.at(i-1).y*100 + 20), Point2i(omegas.at(i).x*250, omegas.at(i).y*100 + 20), Scalar(0, 180, 255));
    }

    imshow("omegas", out);
    //imwrite("/home/nuc/omegas.jpg", out);
    waitKey(1);
}

}
